#include <machine/asmdefs.h>

ASM_HEADER

FNDEFN_BEGIN(cck_55_rpu_asm)

        /* load input, correlation vals  |   set vectors to zero |   set acc0 to zero */
        lr              %vr0,%r8,0      ||      rsubf    %vr4,%vr4,%vr4     ||        lir             %r15,0
        lr              %vr2,%r12,0     ||      rsubf    %vr6,%vr6,%vr6     ||        ctsr            %r15,72

        /* set up alternating I/Q    |   set loop counter to 4 */
        rshf            %vr1,%vr0,0     ||      lir             %r14,4
        rshf            %vr1,%vr0,7     ||      ctsr            %r14,5

        /* calculate 4 pairs of outputs (I and Q put in vr4 and vr6), high and low bits shifted in */
        /* high bits are irrelevent */
LOOP_OUT:
        rxmulredsl      %ac0,%vr0,%vr2
        rxmulredsh      %ac0,%vr0,%vr2  ||      lru      %vr2,%r12,32
        rshift          %vr4,%ac0,4
        rshift          %vr6,%ac0,5     ||      ctsr     %r15,72            ||       loop		0,%lc1,LOOP_OUT

        /* shift values to the beginning of the vectors  | put 1 in upper 16 bits of acc1 */
        rshift          %vr4,%ac0,6
        rshift          %vr4,%ac0,6
        rshift          %vr4,%ac0,0
        rshift          %vr6,%ac0,6     ||      lir     %r14,2
        rshift          %vr6,%ac0,6     ||      slil    %r14,%r14,15
        rshift          %vr6,%ac0,0     ||      ctsr    %r14,73

        /* remove unwanted alternate values (from high bits) */
        rshf1           %vr4,%vr6,3

        /* set up vector of 1s, divide values by 2 (shift right by 1) */
        rbroad          %vr7,%ac1,0
        rsra            %vr4,%vr4,%vr7
        rsra            %vr6,%vr6,%vr7

        /* square sum of I,Q */
        rmuls           %vr5,%vr4,%vr4  ||      str %vr4,%r10,0     ||      ctsr       %r15,0x45        # set mpos to zero
        rmacs           %vr5,%vr6,%vr6  ||      str %vr6,%r11,0     ||      ctsr       %r15,0x44        # set mcount to zero
        rsra            %vr5,%vr5,%vr7

        /* find max, to calculate position: */
        rsearchmax      %ac0,%vr5       ||      str %vr5,%r9,0
        cfsr            %r8,0x45        ||      ji  %jt0,8


FNDEFN_END(cck_55_rpu_asm)


FNDEFN_BEGIN(cck_55_new_rpu_asm)

        /* set stack pointer */
        stwul           %r1,%r1,-96
        addil           %r13,%r1,8

        /* load input, correlation vals  |   set vectors to zero |   set acc0 to zero */
        lr              %vr0,%r8,0      ||      rsubf    %vr4,%vr4,%vr4     ||        lir             %r15,0
        lr              %vr2,%r12,0     ||      rsubf    %vr6,%vr6,%vr6     ||        ctsr            %r15,72

        /* set up alternating I/Q    |   set loop counter to 4 */
        rshf            %vr1,%vr0,0     ||      lir             %r14,4
        rshf            %vr1,%vr0,7     ||      ctsr            %r14,5

        /* calculate 4 pairs of outputs (I and Q put in vr4 and vr6), high and low bits shifted in */
        /* high bits are irrelevent */
LOOP_OUT_NEW:
        rxmulredsl      %ac0,%vr0,%vr2
        rxmulredsh      %ac0,%vr0,%vr2  ||      lru      %vr2,%r12,32
        rshift          %vr4,%ac0,4
        rshift          %vr6,%ac0,5     ||      ctsr     %r15,72            ||       loop		0,%lc1,LOOP_OUT_NEW

        /* shift values to the beginning of the vectors  | put 1 in upper 16 bits of acc1 */
        rshift          %vr4,%ac0,6
        rshift          %vr4,%ac0,6
        rshift          %vr4,%ac0,0
        rshift          %vr6,%ac0,6     ||      lir     %r14,2
        rshift          %vr6,%ac0,6     ||      slil    %r14,%r14,15
        rshift          %vr6,%ac0,0     ||      ctsr    %r14,73

        /* remove unwanted alternate values (from high bits) */
        rshf1           %vr4,%vr6,3

        /* set up vector of 1s, divide values by 2 (shift right by 1) */
        rbroad          %vr7,%ac1,0
        rsra            %vr4,%vr4,%vr7
        rsra            %vr6,%vr6,%vr7

        /* square sum of I,Q */
        rmuls           %vr5,%vr4,%vr4  ||      str %vr4,%r13,0     ||      ctsr       %r15,0x45        # set mpos to zero
        rmacs           %vr5,%vr6,%vr6  ||      str %vr6,%r13,32     ||      ctsr       %r15,0x44        # set mcount to zero
        rsra            %vr5,%vr5,%vr7

        /* find max, to calculate position: */
        rsearchmax      %ac0,%vr5       ||      str %vr5,%r9,0
        cfsr            %r8,0x45

        /* load from temp vector */
        sli             %r15,%r8,1
        add             %r15,%r13,%r15
        lh              %r14,%r15,0
        lhl             %r15,%r15,32

        /* store array values */
        sth             %r14,%r10,0
        sth             %r15,%r11,0

        lw              %r1,%r1,0
        ji              %jt0,8

FNDEFN_END(cck_55_new_rpu_asm)



